//
// Created by nuo on 2021/3/11.
//



// using templates for processPointClouds so also include .cpp to help linker
#include <ros/ros.h>

#include <sensor_msgs/PointCloud2.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include <iostream>

#include <pcl/segmentation/extract_clusters.h>

#include <pcl/visualization/cloud_viewer.h>

#include <stdio.h>





class AddTimeStamp{
private:
    ros::NodeHandle nh;
    ros::Subscriber subLaserCloud;

    ros::Publisher pubLaserCloud;

    pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudIn;
    pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudOut;


public:

    AddTimeStamp(): nh("~")
    {

        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/lslidar_point_cloud", 10000, &AddTimeStamp::cloudHandler, this);

        pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_points", 10000);

        allocateMemory();

    }

    void allocateMemory(){

        laserCloudIn.reset(new pcl::PointCloud<pcl::PointXYZI>());
        laserCloudOut.reset(new pcl::PointCloud<pcl::PointXYZI>());

    }

    void resetParameters(){

        laserCloudIn->clear();
        laserCloudOut->clear();

    }

    ~AddTimeStamp(){}



    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
        std::cout << " copyPointCloud is running...... " << std::endl;

        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
        laserCloudOut->header.stamp = ros::Time::now().toSec();
        laserCloudOut->header.frame_id = "velodyne";
        pcl::PointXYZI point;
        for (int i = 0; i < laserCloudIn->points.size(); ++i) {
            point.x = laserCloudIn->points[i].x;
            point.y = laserCloudIn->points[i].y;
            point.z = laserCloudIn->points[i].z;
            point.intensity = laserCloudIn->points[i].intensity;

            laserCloudOut->points.push_back(point);

        }
        std::cout << " the size of fromROSMsg laserCloudIn is : " << laserCloudIn->points.size() << std::endl;
        std::cout << " the size of fromROSMsg laserCloudIn is : " << laserCloudOut->points.size() << std::endl;
        // Remove Nan points
        /*std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
        std::cout << " the size of removeNaNFromPointCloud laserCloudIn is : " << laserCloudIn->points.size() << std::endl;*/

    }

    void dealdata()
    {

    }


    void PublishData()
    {
        // 1. Publish clouds
        sensor_msgs::PointCloud2 laserCloudTemp;

        pcl::toROSMsg(*laserCloudOut, laserCloudTemp);
        laserCloudTemp.header.stamp = ros::Time().fromSec(laserCloudOut->header.stamp);
        laserCloudTemp.header.frame_id = "velodyne";
        pubLaserCloud.publish(laserCloudTemp);

    }

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);


        if (laserCloudIn->size() != 0){  // if not add this judgement,it will build many viewers
            // 2. deal the data and show in viewer
            //dealdata();

            // 3. Publish data
            PublishData();

            // 4. Reset parameters for next iteration
            resetParameters();
        }

    }

};

// char* argv[] means array of char pointers, whereas char** argv means pointer to a char pointer.
int main(int argc, char **argv) {

    ros::init(argc, argv, "obstacle_detection_node");
    //ros::NodeHandle nh;

    //std::cout << "starting obstacle_detection_node" << std::endl;

    AddTimeStamp ATS;

    ROS_INFO("\033[1;32m---->\033[0m obstacle detection Started.");

    ros::spin();

    return 0;

}
